#include "Transforms.h"

namespace OCPP
{
	namespace Cesium
	{
		using LocalFrame1 = std::map<String, StringMap>;
		LocalFrame1 vectorProductLocalFrame;

		using PositionLocalFrame1 = std::map<String, DoubleVector>;
		PositionLocalFrame1 degeneratePositionLocalFrame;

		using BasicFrameToFixedFrameCallback = std::function<Matrix4(Cartesian3,
			String,
			String,
			String, Ellipsoid)>;
		std::map<String, BasicFrameToFixedFrameCallback> localFrameToFixedFrameCache;

		std::map<String, Cartesian3> scratchCalculateCartesian;


		bool transfromInitialized = false;

		void Transforms::init()
		{
			transfromInitialized = true;
			{
				StringMap up;
				up["south"] = "east";
				up["north"] = "west";
				up["west"] = "south";
				up["east"] = "north";
				vectorProductLocalFrame["east"] = up;
			}
			{
				StringMap down;
				down["south"] = "west";
				down["north"] = "east";
				down["west"] = "north";
				down["east"] = "south";
				vectorProductLocalFrame["down"] = down;
			}
			{
				StringMap south;
				south["up"] = "west";
				south["down"] = "east";
				south["west"] = "down";
				south["east"] = "up";
				vectorProductLocalFrame["south"] = south;
			}
			{
				StringMap north;
				north["up"] = "east";
				north["down"] = "west";
				north["west"] = "up";
				north["east"] = "down";
				vectorProductLocalFrame["north"] = north;
			}
			{
				StringMap west;
				west["up"] = "north";
				west["down"] = "south";
				west["north"] = "down";
				west["south"] = "up";
				vectorProductLocalFrame["west"] = west;
			}
			{
				StringMap east;
				east["up"] = "south";
				east["down"] = "north";
				east["north"] = "up";
				east["south"] = "down";
				vectorProductLocalFrame["east"] = east;
			}

			degeneratePositionLocalFrame["north"] = { -1,0,0 };
			degeneratePositionLocalFrame["east"] = { 0,1,0 };
			degeneratePositionLocalFrame["up"] = { 0,0,1 };
			degeneratePositionLocalFrame["south"] = { 1,0,0 };
			degeneratePositionLocalFrame["west"] = { 0,-1,0 };
			degeneratePositionLocalFrame["down"] = { 0,0,-1 };
		}

		BasicFrameToFixedFrameCallback localFrameToFixedFrameGenerator(String firstAxis, String secondAxis) {
			String thirdAxis = vectorProductLocalFrame[firstAxis][secondAxis];

			/**
			 * Computes a 4x4 transformation matrix from a reference frame
			 * centered at the provided origin to the provided ellipsoid's fixed reference frame.
			 * @callback Transforms.LocalFrameToFixedFrame
			 * @param {Cartesian3} origin The center point of the local reference frame.
			 * @param {Ellipsoid} [ellipsoid=Ellipsoid.WGS84] The ellipsoid whose fixed frame is used in the transformation.
			 * @param {Matrix4} [result] The object onto which to store the result.
			 * @returns {Matrix4} The modified result parameter or a new Matrix4 instance if none was provided.
			 */
			BasicFrameToFixedFrameCallback resultat;
			auto hashAxis = firstAxis + secondAxis;

			if (localFrameToFixedFrameCache.find(hashAxis) != localFrameToFixedFrameCache.end()) {
				resultat = localFrameToFixedFrameCache[hashAxis];
			}
			else {
				resultat = [](Cartesian3 origin,
					String firstAxis,
					String secondAxis,
					String thirdAxis,
					Ellipsoid ellipsoid = Ellipsoid::WGS84) {

						static std::map<String, Cartesian3> scratchCalculateCartesian;
						scratchCalculateCartesian["east"] = Cartesian3();
						scratchCalculateCartesian["north"] = Cartesian3();
						scratchCalculateCartesian["up"] = Cartesian3();
						scratchCalculateCartesian["west"] = Cartesian3();
						scratchCalculateCartesian["south"] = Cartesian3();
						scratchCalculateCartesian["down"] = Cartesian3();

						Cartesian3 scratchFirstCartesian = Cartesian3();
						Cartesian3 scratchSecondCartesian = Cartesian3();
						Cartesian3 scratchThirdCartesian = Cartesian3();

						Matrix4 result;
						if (
							Cartesian3::equalsEpsilon(origin, Cartesian3::ZERO, CesiumMath::EPSILON14)
							) {
							// If x, y, and z are zero, use the degenerate local frame, which is a special case
							auto scratchFirstCartesian = Cartesian3::unpack(
								degeneratePositionLocalFrame[firstAxis],
								0
							);
							auto scratchSecondCartesian = Cartesian3::unpack(
								degeneratePositionLocalFrame[secondAxis],
								0
							);
							auto scratchThirdCartesian = Cartesian3::unpack(
								degeneratePositionLocalFrame[thirdAxis],
								0
							);
						}
						else if (
							CesiumMath::equalsEpsilon(origin.x, 0.0, CesiumMath::EPSILON14) &&
							CesiumMath::equalsEpsilon(origin.y, 0.0, CesiumMath::EPSILON14)
							) {
							// If x and y are zero, assume origin is at a pole, which is a special case.
							auto sign = CesiumMath::sign(origin.z);

							auto scratchFirstCartesian = Cartesian3::unpack(
								degeneratePositionLocalFrame[firstAxis],
								0
							);
							if (firstAxis != "east" && firstAxis != "west") {
								scratchFirstCartesian = Cartesian3::multiplyByScalar(
									scratchFirstCartesian,
									sign
								);
							}

							auto scratchSecondCartesian = Cartesian3::unpack(
								degeneratePositionLocalFrame[secondAxis],
								0
							);
							if (secondAxis != "east" && secondAxis != "west") {
								scratchSecondCartesian = Cartesian3::multiplyByScalar(
									scratchSecondCartesian,
									sign
								);
							}

							auto scratchThirdCartesian = Cartesian3::unpack(
								degeneratePositionLocalFrame[thirdAxis],
								0
							);
							if (thirdAxis != "east" && thirdAxis != "west") {
								scratchThirdCartesian = Cartesian3::multiplyByScalar(
									scratchThirdCartesian,
									sign
								);
							}
						}
						else {
							scratchCalculateCartesian["up"] =
								ellipsoid.geodeticSurfaceNormal(origin);

							scratchCalculateCartesian["east"].x = -origin.y;
							scratchCalculateCartesian["east"].y = origin.x;
							scratchCalculateCartesian["east"].z = 0.0;
							scratchCalculateCartesian["east"] = Cartesian3::normalize(scratchCalculateCartesian["east"]);
							scratchCalculateCartesian["north"]
								= Cartesian3::cross(scratchCalculateCartesian["up"],
									scratchCalculateCartesian["east"]);

							scratchCalculateCartesian["down"]
								= Cartesian3::multiplyByScalar(
									scratchCalculateCartesian["up"],
									-1
								);
							auto scratchCalculateCartesian_west
								= Cartesian3::multiplyByScalar(
									scratchCalculateCartesian["east"],
									-1
								);
							auto scratchCalculateCartesian_south
								= Cartesian3::multiplyByScalar(
									scratchCalculateCartesian["north"],
									-1
								);

							scratchFirstCartesian = scratchCalculateCartesian[firstAxis];
							scratchSecondCartesian = scratchCalculateCartesian[secondAxis];
							scratchThirdCartesian = scratchCalculateCartesian[thirdAxis];
						}
						result[0] = scratchFirstCartesian.x;
						result[1] = scratchFirstCartesian.y;
						result[2] = scratchFirstCartesian.z;
						result[3] = 0.0;
						result[4] = scratchSecondCartesian.x;
						result[5] = scratchSecondCartesian.y;
						result[6] = scratchSecondCartesian.z;
						result[7] = 0.0;
						result[8] = scratchThirdCartesian.x;
						result[9] = scratchThirdCartesian.y;
						result[10] = scratchThirdCartesian.z;
						result[11] = 0.0;
						result[12] = origin.x;
						result[13] = origin.y;
						result[14] = origin.z;
						result[15] = 1.0;
						return result;
					};
			}
			localFrameToFixedFrameCache[hashAxis] = resultat;
			return resultat;
		};

		Matrix4 Transforms::eastNorthUpToFixedFrame(
			Cartesian3 cartesian3,
			Ellipsoid ellipsoid) {
			if (!transfromInitialized) Transforms::init();
			String firstAxis = "east";
			String secondAxis = "north";
			String thirdAxis = vectorProductLocalFrame[firstAxis][secondAxis];

			auto f = localFrameToFixedFrameGenerator(
				firstAxis,
				secondAxis);
			return f(cartesian3, firstAxis, secondAxis, thirdAxis, ellipsoid);
		}
	}
}

